﻿using Schism.Mapping;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using DeenGames.Utils.AStarPathFinder;
using Microsoft.Xna.Framework;
using Schism.Core;

namespace Schism.AI
{
    public static class AStarPathFinder
    {
        public static byte[,] GetMap(CellMap map)
        {
            // return the cell maps map, ensuring that the size is a power of two
            int newWidth = PathFinderHelper.RoundToNearestPowerOfTwo(map.MapWidth);
            int newHeight = PathFinderHelper.RoundToNearestPowerOfTwo(map.MapHeight);

            byte[,] temp = new byte[newHeight, newWidth];

            for (int y = 0; y < newHeight; y++)
            {
                for (int x = 0; x < newWidth; x++)
                {
                    //Should be blocked if it doesnt exist
                    if (x > map.MapWidth - 1 || y > map.MapHeight - 1)
                        temp[y, x] = PathFinderHelper.BLOCKED_TILE;
                    else
                        temp[y, x] = (byte)(map[x, y].IsWalkable ? PathFinderHelper.EMPTY_TILE : PathFinderHelper.BLOCKED_TILE);
                }
            }

            return temp;
        }

        public static IEnumerable<Waypoint> FindPathBetween(DungeonPos start, DungeonPos finish, CellMap map)
        {
            DeenGames.Utils.AStarPathFinder.PathFinderFast pfinder = new PathFinderFast(GetMap(map));
            pfinder.Diagonals = false;
            List<PathFinderNode> path = pfinder.FindPath(
                new DeenGames.Utils.Point(start.MapX, start.MapY),
                new DeenGames.Utils.Point(finish.MapX, finish.MapY));

            if (path == null)
                return new List<Waypoint>(); //No path found;

            path.Reverse(); // Reverse
            return TrimDown(path.Select(p => new Vector2(p.Y, p.X)).ToList()).Select(x => new Waypoint(x));
        }


        public static IEnumerable<Vector2> TrimDown(List<Vector2> nodes)
        {
            List<Vector2> finalList = new List<Vector2>();
            Vector2 moveDirection = Vector2.Zero;

	        for(int i =0 ; i < nodes.Count(); i++)
	        {
                if (i == 0)
                {
                    finalList.Add(nodes[i]);
                    moveDirection = nodes[i + 1] - nodes[i];
                }
                else if (i + 1 >= nodes.Count())
                {
                    finalList.Add(nodes[i]);
                    continue;
                }
			
		        Vector2 me, next;
		        me = nodes[i];
                next = nodes[i + 1];

                Vector2 mymove = next - me;

                if (mymove != moveDirection)
                {
                    finalList.Add(nodes[i]);
                    moveDirection = mymove;
                }
	        }
            return finalList;
        }
    }
}
